//
// Created by cch on 2022/5/1.
//

#ifndef IMU_GPS_EKF_GPS_PROCESSER_H
#define IMU_GPS_EKF_GPS_PROCESSER_H

#include "base_type.h"
#include "utils.h"

namespace IMUGPS {
class GpsProcesser {
public:
  GpsProcesser(const Eigen::Vector3d &I_P_GPS);
  void Update(const Eigen::Vector3d &init_lla, const GpsDataPtr &gpsDataPtr,
              State *state);
  void computeResidualAndJacobian(const Eigen::Vector3d &init_lla,
                                  Eigen::Matrix<double, 3, 15> &jacobian,
                                  Eigen::Vector3d &residual, const State *state,
                                  const GpsDataPtr &gpsDataPtr);
  static void AddDeltaToState(const Eigen::Matrix<double, 15, 1> &delta_x,
                              State *state);

private:
  const Eigen::Vector3d I_P_GPS_;
};
} // namespace IMUGPS

#endif // IMU_GPS_EKF_GPS_PROCESSER_H
